#include "GPSModule.h"
#include "..\System\F2806x_Device.h"
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

bool GPSModule::Initialize()
{
	if( Init(9600) == false ) return false;
	
	// switch to 38k
	// send binary data
	unsigned char data[30];
	data[0] = 0xB5; // sync1
	data[1] = 0x62; // sync2
	data[2] = 0x06; // CFG
	data[3] = 0x00; // PRT
	data[4] = 0x14; // len-low
	data[5] = 0x00; // len-high

	data[6] = 0x01; // port1
	data[7] = 0x00; // res
	data[8] = 0x00; // res
	data[9] = 0x00; // res

	data[10] = 0xD0; // 8bit
	data[11] = 0x08; // no parity
	data[12] = 0x00; // 0 stop
	data[13] = 0x00; // res

	data[14]= 0x00; // baud - low
	data[15]= 0x96; // baud - high
	data[16]= 0x00; // baud
	data[17]= 0x00; // baud

	data[18]= 0x03; // in proto
	data[19]= 0x00; // res
	data[20]= 0x03; // out proto
	data[21]= 0x00; // res

	data[22]= 0x00; // res
	data[23]= 0x00; // res
	data[24]= 0x00; // res
	data[25]= 0x00; // res

	//data[26]= 0xDA; // chk-a
	//data[27]= 0xA9; // chk-b

	// calculate checksum
	Uint8 chk_a = 0;
	Uint8 chk_b = 0;
	for(int i=0; i!=24; i++)
	{
		chk_a = chk_a + data[i+2];
		chk_b = chk_b + chk_a;
	}

	data[26]= chk_a & 0x00FF; // chk-a
	data[27]= chk_b & 0x00FF; // chk-b

	for(int i=0; i!=3; i++)
	{
		SendData((char*)data, 28);
		for(Uint32 i=0; i!= 200000; i++);
	}
	
	// switch port to 38k
	ScibRegs.SCICTL1.all =0x0003;  // enable TX, RX, internal SCICLK, Disable RX ERR, SLEEP, TXWAKE
	ScibRegs.SCIHBAUD    =0x0000;  // switch to 0x79 for 57600
	ScibRegs.SCILBAUD    =0x0040;
	ScibRegs.SCICTL1.all =0x0023;  // Relinquish SCI from Reset
	
	// Set rate to 5HZ!!!
	data[0] = 0xB5; // sync1
	data[1] = 0x62; // sync2
	data[2] = 0x06; // CFG
	data[3] = 0x08; // RATE
	data[4] = 0x06; // len-low
	data[5] = 0x00; // len-high

	data[6] = 0xC8; // 200ms
	data[7] = 0x00; // 200ms

	data[8] = 0x01; // 1 cycle nav rate
	data[9] = 0x00; // res

	data[10] = 0x01; // 1 - GPS TIME
	data[11] = 0x00; // res

	data[12]= 0xDE; // chk-a
	data[13]= 0x6A; // chk-b

	SendData((char*)data, 14);


	// reset values
	RXBufPosition = 0;
	m_time[0] = 0;
	m_latitude[0] = 0;
	m_longitude[0] = 0;
	m_fixStatus = 0;
	m_satUsed = 0;
	m_alt = 0;
	m_VelX = 0;
	m_VelY = 0;
	m_NS = 'S';
	m_EW = 'W';
	m_Lat = 0;
	m_Long = 0;
	m_MessageCount = 0;
	m_ErrorCount = 0;
	m_NewReceived = false;
	
	return true;
}

void GPSModule::GetCoordinates(char* buffer)
{
	sprintf(buffer, "Lat: %s%c, Long: %s%c, Alt: %f", m_latitude, m_NS, m_longitude, m_EW, m_alt);	
}

void GPSModule::GetFixInfo(char* buffer)
{
	sprintf(buffer, "Status: %d, Satellites: %d, Time: %s", m_fixStatus, m_satUsed, m_time);
}

void GPSModule::GetTime(char* buffer)
{
	strcpy(buffer, m_time);
}

float64 GPSModule::GetLatitude()
{
	return m_Lat;
}

float64 GPSModule::GetLongitude()
{
	return m_Long;
}

float GPSModule::GetAltitude()
{
	return m_alt;
}

int GPSModule::GetSatUsed()
{
	return m_satUsed;
}

float GPSModule::GetVelX()
{
	return m_VelX;
}

float GPSModule::GetVelY()
{
	return m_VelY;
}

bool GPSModule::GetReceivedAutoreset()
{
	bool newrecv = m_NewReceived;
	m_NewReceived = false;

	return newrecv;
}
									
void GPSModule::Update()
{
	char buff[250]; // must be called at min rate of 20Hz! (50ms) (4800bytes/second)
	Uint32 len = GetData(buff);

	// parse data
	if( len > 0 )
	{
		buff[len] = 0;
		ParseCommand(buff);
	}		
}

int GPSModule::ParseCommand(char* input) // returns message length
{
	for(int i=0; i!= strlen(input); i++)
	{
		char data = input[i];
		if( data == '\r' )
		{
			// end of command
			int size = RXBufPosition;
			RXBufPosition = 0;
			RXBuffer[size] = 0; // null terminate

			if( CheckChecksum(RXBuffer) == true )
			{			
				m_MessageCount++;
				ExecuteCommand(RXBuffer, size);
			}
			else
			{
				m_ErrorCount++;
			}
		}
		else
		{
			if( RXBufPosition >= 200 || data == '$')
			{
				// overrun or RESET
				RXBufPosition = 0;
			}
			RXBuffer[RXBufPosition] = data;
			RXBufPosition++;
		}
	}
	return 0;
}

void GPSModule::ExecuteCommand(char* input, int size)
{
	// parse command
	char* token = strtok( input, ",");
	if( !strcmp(token, "$GPGGA") )
	{		
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// UTC time
			strcpy(m_time, token);
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Latitude
			strcpy(m_latitude, token);

			// convert lat
			float64 num;
			sscanf(m_latitude, "%Lf", &num );
			int deg = (int)floor(num/100);
			float64 min = num-deg*100;
			float64 total = deg + min/60;
			m_Lat = total;
		} 
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// N/S
			m_NS = token[0];
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Longitude
			strcpy(m_longitude, token);

			// convert lon
			float64 num;
			sscanf(m_longitude, "%Lf", &num );
			int deg = (int)floor(num/100);
			float64 min = num-deg*100;
			float64 total = deg + min/60;
			m_Long = total;
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// E/W
			m_EW = token[0];
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Position Fix
			m_fixStatus = atoi(token);
			
			if( m_fixStatus == 0 ) m_satUsed = 0; // no fix
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			if( token[0] == '0' ) token++;

			// Sat Used
			m_satUsed = atoi(token);
		}
		else
		{
			m_satUsed = 0; // no fix/errors
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// HDOP
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// msl
			m_alt = atof(token);
		}
	}

	// $GPVTG,,,,,,,,,N*30
	// $GPVTG,,T,,M,0.144,N,0.267,K,A*21
	// $GPVTG,77.52,T,,M,0.004,N,0.008,K,A*06
	if( !strcmp(token, "$GPVTG") )
	{
		m_NewReceived = true;

		bool isValid = false;

		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Course over ground (true)
			m_COG = atof(token);
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Fixed field: true
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Fixed field: magnetic
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Speed over ground [knot]->ignore
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Fixed field: knots
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Speed over ground
			m_SOG = atof(token); // [kph]
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Fixed field: kilometers per hour
		}
		token = strtok(NULL, ",");
		if( token != NULL )
		{
			// Mode Indicator
			if( token[0] == 'A' ) isValid = true; // no FIX
		}

		if( isValid )
		{
			// calculate Velocity
			float velMpS = m_SOG/3.6f; // to m/s

			// rotate
			float angleRads = m_COG/180*3.14159f;
			m_VelX = cos(angleRads) * velMpS;
			m_VelY = sin(angleRads) * velMpS;
		}
		else
		{
			m_VelX = 0;
			m_VelY = 0;
		}
	}
}

unsigned char GPSModule::CalculateChecksum(char* str, int len)
{
	unsigned char checksum = 0;

	for(int i=0; i!= len; i++ )
	{
		checksum = checksum^str[i];
	}

	return checksum;
}

bool GPSModule::CheckChecksum(char* str)
{
	int len = strlen(str);
	if(str[0] != '$') return false; // invalid start character
	
	for(int starpos=0; starpos!=len-2; starpos++ )
	{
		if(str[starpos] == '*' )
		{
			unsigned char calcChecksum = CalculateChecksum(&str[1], starpos-1);
			unsigned int checkSum;
			sscanf(&str[starpos+1], "%x", &checkSum);
			bool valid = (checkSum == calcChecksum);

			return valid;
		}
	}

	return false;
}
